using System;
using System.Drawing;
using Diversity.Maths;

namespace Diversity.Robotics.Navigation
{
    /// <summary>
    /// An x,y,theta representation of a differential drive state.
    /// </summary>
    public class Pose2D : ICloneable
    {
        private double _x;
        private double _y;
        private double _theta;

        /// <summary>
        /// Creates an empty state
        /// </summary>
        public Pose2D()
        {
        }

        /// <summary>
        /// Create a differential drive state
        /// </summary>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <param name="theta"></param>
        public Pose2D(double x, double y, double theta)
        {
            _x = x;
            _y = y;
            _theta = theta;
        }

        /// <summary>
        /// The real X coordinate
        /// </summary>
        public double X
        {
            get { return _x; }
            set { _x = value; }
        }

        /// <summary>
        /// The real Y coordinate
        /// </summary>
        public double Y
        {
            get { return _y; }
            set { _y = value; }
        }

        /// <summary>
        /// The rotation in Radians
        /// </summary>
        public double Theta
        {
            get { return _theta; }
            set { _theta = value; }
        }

        /// <summary>
        /// Creates a copy of this state
        /// </summary>
        /// <returns></returns>
        public object Clone()
        {
            return new Pose2D(_x, _y, _theta);
        }

        #region Convertion
        /// <summary>
        /// Creates a string representation of the state
        /// </summary>
        /// <returns></returns>
        public override string ToString()
        {
            return "State (x: " + _x + " ,y: " + _y + " ,Theta: " + _theta + ")";
        }

        /// <summary>
        /// Creates an int point structure for drawing
        /// </summary>
        /// <returns></returns>
        public Point ToPoint()
        {
            return new Point((int)Math.Round(_x),(int)Math.Round(_y));
        }

        /// <summary>
        /// Creates a float point structure for drawing
        /// </summary>
        /// <returns></returns>
        public PointF ToPointF()
        {
            return new PointF((float)_x, (float)_y);
        }

        /// <summary>
        /// Creates a simple 3*1 matrix
        /// </summary>
        /// <returns></returns>
        public Matrix ToMatrix()
        {
            Matrix returnMatrix = new Matrix(1, 3);
            returnMatrix[0, 0] = _x;
            returnMatrix[0, 1] = _y;
            returnMatrix[0, 2] = _theta;
            return returnMatrix;
        }
        #endregion

        #region Operators

        /// <summary>
        /// Returns true if the sub values are numerically equal
        /// </summary>
        /// <param name="state"></param>
        /// <returns></returns>
        public bool IsEqualTo(Pose2D state)
        {
            if (state.X != _x || state.Y != _y || state.Theta != _theta)
            {
                return false;
            } else
            {
                return true;
            }
        }

        /// <summary>
        /// Less than 0.5 cm or 1 degree
        /// </summary>
        /// <param name="state"></param>
        /// <returns></returns>
        public bool IsAlmostEqualTo(Pose2D state)
        {
            double oneDegreeAsRadians = 0.017;
            double maxDistance = 0.1; // 1mm in cm
            if (Math.Abs(state.X - _x) > maxDistance || Math.Abs(state.Y - _y) > maxDistance || Math.Abs(state.Theta - _theta) > oneDegreeAsRadians)
            {
                return false;
            }
            else
            {
                return true;
            }
        }

        /// <summary>
        /// Calculates the Euclidian distance between two points
        /// </summary>
        /// <param name="state2"></param>
        /// <returns></returns>
        public double DistanceTo(Pose2D state2)
        {
            return this > state2;
        }

        /// <summary>
        /// Calculates the Euclidian distance between two points
        /// </summary>
        /// <param name="state1"></param>
        /// <param name="state2"></param>
        /// <returns>The Euclidian distance</returns>
        public static double operator >(Pose2D state1, Pose2D state2)
        {
            return Math.Sqrt(Math.Pow(state1.X - state2.X,2) + Math.Pow(state1.Y - state2.Y,2));
        }

        /// <summary>
        /// Calculates the Euclidian distance between two points
        /// </summary>
        /// <param name="state1"></param>
        /// <param name="state2"></param>
        /// <returns>The Euclidian distance</returns>
        public static double operator <(Pose2D state1, Pose2D state2)
        {
            return state1 > state2;
        }

        /// <summary>
        /// Subtracts one state from another
        /// </summary>
        /// <param name="state1"></param>
        /// <param name="state2"></param>
        /// <returns></returns>
        public static Pose2D operator -(Pose2D state1, Pose2D state2)
        {
            return new Pose2D(state1.X - state2.X, state1.Y - state2.Y, state1.Theta - state2.Theta);
        }

        /// <summary>
        /// Subtracts one state from another
        /// </summary>
        /// <param name="state2"></param>
        /// <returns></returns>
        public Pose2D Subtract(Pose2D state2)
        {
            return this - state2;
        }

        /// <summary>
        /// Adds a state to another
        /// </summary>
        /// <param name="state2"></param>
        /// <returns></returns>
        public Pose2D Add(Pose2D state2)
        {
            return this + state2;
        }

        /// <summary>
        /// Adds a matrix to a state
        /// </summary>
        /// <param name="matrix"></param>
        /// <returns></returns>
        public Pose2D Add(Matrix matrix)
        {
            return this + matrix;
        }

        /// <summary>
        /// Adds two states together
        /// </summary>
        /// <param name="state1"></param>
        /// <param name="state2"></param>
        /// <returns></returns>
        public static Pose2D operator +(Pose2D state1, Pose2D state2)
        {
            return  new Pose2D(state1.X + state2.X, state1.Y + state2.Y, state1.Theta + state2.Theta);    
        }

        /// <summary>
        /// Adds a state to a matrix
        /// </summary>
        /// <param name="state1"></param>
        /// <param name="matrix"></param>
        /// <returns></returns>
        public static Pose2D operator +(Pose2D state1, Matrix matrix)
        {
            return new Pose2D(state1.X + matrix[0, 0], state1.Y + matrix[1, 0], state1.Theta + matrix[2, 0]);
        }

        /// <summary>
        /// Subtracts a matrix from a state
        /// </summary>
        /// <param name="state1"></param>
        /// <param name="matrix"></param>
        /// <returns></returns>
        public static Pose2D operator -(Pose2D state1, Matrix matrix)
        {
            return new Pose2D(state1.X - matrix[0, 0], state1.Y - matrix[1, 0], state1.Theta - matrix[2, 0]);
        }

        /// <summary>
        /// Returns the angle between the two states
        /// </summary>
        /// <param name="state1">Start state</param>
        /// <param name="state2">End state</param>
        /// <returns>The angle between the two states</returns>
        public static double operator %(Pose2D state1, Pose2D state2)
        {
            return Math.Atan2(state1.Y - state2.Y, state1.X - state2.X);
        }

        /// <summary>
        /// Calculates the average of two poses
        /// </summary>
        /// <param name="state1"></param>
        /// <param name="state2"></param>
        /// <returns></returns>
        public static Pose2D Average(Pose2D state1, Pose2D state2)
        {
            // TODO check angles
            double x = (state1.X + state2.X)/2;
            double y = (state1.Y + state2.Y)/2;
            double theta = (state1.Theta + state2.Theta)/2;
            return new Pose2D(x,y,theta);
        }
        
        #endregion

    }
}
